#ifndef __EM_PLANNER_SPEED_PLANNER_HPP__
#define __EM_PLANNER_SPEED_PLANNER_HPP__

#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense>

#include "./em_utils.hpp"

using namespace std;
using namespace Eigen;

class SpeedPlanner
{
private:
    /* data */
    Location plan_start_;
    SLPoint plan_start_sl_;
    vector<Location> dp_path_;
    vector<SLPoint> dp_path_sl_;
    // 速度规划起始信息
    STInfo start_;

    /* 动态规划变量 */
    vector<double> s_list;
    vector<double> t_list;
public:
    SpeedPlanner(/* args */);
    SpeedPlanner(const Location &plan_start, const SLPoint &plan_start_sl, 
                 const vector<Location> &dp_path, const vector<SLPoint> &dp_path_sl);
    ~SpeedPlanner();
    bool genSTMap(const vector<Location> &obs_vec, const vector<SLPoint> &obs_vec_sl, vector<STMap> &st_vec);

    bool dp_decision(const vector<STMap> &st_vec, vector<double> &s_vec, vector<double> &v_vec, vector<double> &t_vec);

    STInfo calcSTCoordinate(const int col, const int row);
    double calcuDpCost(const STInfo &begin, const STInfo &end, 
                    const vector<STMap> &st_vec);
    double calcuObsCost(const STInfo &begin, const STInfo &end, 
                        const vector<STMap> &st_vec, double w_cost_obs);
    double calcuCollisionCost(double w_cost_obs, double min_dis);
    void calcuSDotAndDDot(const STInfo &begin, STInfo &end);
};



#endif